/**
 ******************************************************************************
 * Copyright (c) 2022 - ~, SCUT-RobotLab Development Team
 * @file    LegController.cpp
 * @author
 * @brief   Code for .
 * @date   2022-02-28
 * @version 1.0
 * @par Change Log:
 * <table>
 * <tr><th>Date        <th>Version  <th>Author     <th>Description
 * </table>
 *
 ******************************************************************************
 * @attention
 *
 * if you had modified this file, please make sure your code does not have many
 * bugs, update the version Number, write dowm your name and the date, the most
 * important is make sure the users will have clear and definite understanding
 * through your new brief.
 *
 * <h2><center>&copy; Copyright (c) 2022 - ~,SCUT-RobotLab Development Team.
 * All rights reserved.</center></h2>
 ******************************************************************************
 */
#include "LegController.h"

#define pi 3.14159

/*!
 * Zero the leg command so the leg will not output torque
 */
template <typename T>
void LegControllerCommand<T>::zero() {
    tauFeedForward = Vec3<T>::Zero();
    forceFeedForward = Vec3<T>::Zero();
    qDes = Vec3<T>::Zero();
    qdDes = Vec3<T>::Zero();
    pDes = Vec3<T>::Zero();
    vDes = Vec3<T>::Zero();
    kpCartesian = Mat3<T>::Zero();
    kdCartesian = Mat3<T>::Zero();
    kpJoint = Mat3<T>::Zero();
    kdJoint = Mat3<T>::Zero();
}

/*!
 * Zero the leg data
 */
template <typename T>
void LegControllerData<T>::zero() {
    q = Vec3<T>::Zero();
    qd = Vec3<T>::Zero();
    p = Vec3<T>::Zero();
    v = Vec3<T>::Zero();
    J = Mat3<T>::Zero();
    tauEstimate = Vec3<T>::Zero();
}

template <typename T>
void LegController<T>::setControlMode(Ctrl_Mode mode){
    switch (mode)
    {
        case All_Tau:
            ctrl_mode[0] = Mode_Tau; 
            ctrl_mode[1] = Mode_Tau; 
            ctrl_mode[2] = Mode_Tau; 
            break;
        case All_Pos:
            ctrl_mode[0] = Mode_Pos; 
            ctrl_mode[1] = Mode_Pos; 
            ctrl_mode[2] = Mode_Pos; 
            break;
        case Abad_Pos:
            ctrl_mode[0] = Mode_Pos; 
            ctrl_mode[1] = Mode_Tau; 
            ctrl_mode[2] = Mode_Tau; 
            break;    
        default:
            break;
    }
}
/*!
 * Zero all leg commands.  This should be run *before* any control code, so if
 * the control code is confused and doesn't change the leg command, the legs
 * won't remember the last command.
 */
template <typename T>
void LegController<T>::zeroCommand() {
    for (auto& cmd : commands) {
        cmd.zero();
    }
    setEnabled(false);
}

/*!
 * Set the leg to edamp.  This overwrites all command data and generates an
 * emergency damp command using the given gain. For the mini-cheetah, the edamp
 * gain is Nm/(rad/s), and for the Cheetah 3 it is N/m. You still must call
 * updateCommand for this command to end up in the low-level command data!
 */
template <typename T>
void LegController<T>::edampCommand(T gain) {
    zeroCommand();
    for (int leg = 0; leg < 4; leg++) {
        for (int axis = 0; axis < 3; axis++) {
            commands[leg].kdCartesian(axis, axis) = gain;
        }
    }
}

/*!
 * Update the "leg data" from a SPIne board message
 */
template <typename T>
void LegController<T>::updateData(const LegData* robotData) {
    for (int leg = 0; leg < 4; leg++) {
        // q:
        datas[leg].q(0) = robotData->q_abad[leg];
        datas[leg].q(1) = robotData->q_hip[leg];
        datas[leg].q(2) = robotData->q_knee[leg];

        // qd
        datas[leg].qd(0) = robotData->qd_abad[leg];
        datas[leg].qd(1) = robotData->qd_hip[leg];
        datas[leg].qd(2) = robotData->qd_knee[leg];

        // J and p
        computeLegJacobianAndPosition<T>(_quadruped, datas[leg].q, &(datas[leg].J), &(datas[leg].p));

        // v
        datas[leg].v = datas[leg].J * datas[leg].qd;
        
    }
}

/*!
 * Update the "leg command" for the robot
 */
template <typename T>
void LegController<T>::updateCommand(RobotCommand* robotCommand) {
    Vec3<T> footForce_Rcord;
    for (int leg = 0; leg < 4; leg++) {
        // tauFF
        Vec3<T> legTorque = commands[leg].tauFeedForward;

        // forceFF
        Vec3<T> footForce =  commands[leg].forceFeedForward;
        
        // cartesian PD
        footForce += commands[leg].kpCartesian * (commands[leg].pDes - datas[leg].p);
        footForce += commands[leg].kdCartesian * (commands[leg].vDes - datas[leg].v);

        if(leg == 2) footForce_Rcord = footForce;

        // Torque
        legTorque += datas[leg].J.transpose() * footForce;

        // set command:
        robotCommand->tau_abad_ff[leg] = legTorque(0);
        robotCommand->tau_hip_ff[leg] = legTorque(1);
        robotCommand->tau_knee_ff[leg] = legTorque(2);        

        // joint space pd
        robotCommand->kd_abad[leg] = commands[leg].kdJoint(0, 0);
        robotCommand->kd_hip[leg] = commands[leg].kdJoint(1, 1);
        robotCommand->kd_knee[leg] = commands[leg].kdJoint(2, 2);

        robotCommand->kp_abad[leg] = commands[leg].kpJoint(0, 0);
        robotCommand->kp_hip[leg] = commands[leg].kpJoint(1, 1);
        robotCommand->kp_knee[leg] = commands[leg].kpJoint(2, 2);

        robotCommand->q_des_abad[leg] = commands[leg].qDes(0);
        robotCommand->q_des_hip[leg] = commands[leg].qDes(1);
        robotCommand->q_des_knee[leg] = commands[leg].qDes(2);

        robotCommand->qd_des_abad[leg] = commands[leg].qdDes(0);
        robotCommand->qd_des_hip[leg] = commands[leg].qdDes(1);
        robotCommand->qd_des_knee[leg] = commands[leg].qdDes(2);
        
        // estimate torque
        datas[leg].tauEstimate = legTorque + commands[leg].kpJoint * (commands[leg].qDes - datas[leg].q) +
                                 commands[leg].kdJoint * (commands[leg].qdDes - datas[leg].qd);

        robotCommand->flags[leg] = _legsEnabled[leg] ? 1 : 0;

        // Choose control mode
        robotCommand->mode_abad[leg] =  ctrl_mode[0];
        robotCommand->mode_hip[leg] = ctrl_mode[1];
        robotCommand->mode_knee[leg] = ctrl_mode[2];
    }    
}

template struct LegControllerCommand<double>;
template struct LegControllerCommand<float>;

template struct LegControllerData<double>;
template struct LegControllerData<float>;

template class LegController<double>;
template class LegController<float>;

/**
 * @brief Restrict Angle into -1.57~1.57
 *
 * @param input
 * @return float
 */
template <typename T>
T restrictAngle(T input) {
    if (input > 1.57f) {
        return 3.14 - input;
    } else if (input < -1.57f) {
        return -3.14 + input;
    } else
        return input;
}

/**
 * @brief  Compute the auxiliary angle of the foot joint
 */
template <typename T>
void computeAuxiliaryAngle(Quadruped<T>& quad, Vec3<T>& p, Vec3<T>* q) {
    // computer coordinate is different of body coordinate
    T x = p[0];
    T y = -p[2];
    T z = p[1];
    T leg_a = quad._shortLinkLength;
    T leg_b = quad._longLinkLength;
    T leg_c = quad._ExtendLinkLength;

    T Lod2 = pow(x, 2) + pow(y, 2) + pow(z, 2);
    T Loc2 = (pow(leg_a, 2) * leg_c - pow(leg_c, 2) * leg_b - pow(leg_b, 2) * leg_c +
              leg_b * (pow(x, 2) + pow(y, 2) + pow(z, 2))) /
             (leg_b + leg_c);

    // compute alpha
    T alpha = acos((Loc2 + pow(leg_a, 2) - pow(leg_b, 2)) / (2 * leg_a * sqrt(Loc2)));
    q->operator()(1) = restrictAngle(alpha);

    // compute beta
    T angle_DOy = atan2(x, y);
    T theta = acos((pow(leg_a, 2) + Lod2 - pow((leg_b + leg_c), 2)) / (2 * leg_a * sqrt(Lod2)));
    T phi = theta - angle_DOy;
    T m = leg_b * x / (leg_b + leg_c) + leg_c * leg_a * (-sin(phi)) / (leg_b + leg_c);
    T n = leg_b * y / (leg_b + leg_c) + leg_c * leg_a * cos(phi) / (leg_b + leg_c);
    q->operator()(2) = atan2(m, n);

    // compute gamma
    q->operator()(0) = atan2(z, y);
}

template <typename T>
void computeLegAngle(Quadruped<T>& quad, Vec3<T>& p, Vec3<T>* q){
    Vec3<T> q_temp;
    computeAuxiliaryAngle(quad, p, &q_temp);
    q->operator()(0) = q_temp(0);
    q->operator()(1) = q_temp(1) + q_temp(2);
    q->operator()(2) = q_temp(2) - q_temp(1);
}

/*!
 * Compute the position of the foot and its Jacobian.  This is done in the local
 * leg coordinate system. If J/p are NULL, the calculation will be skipped.
 */
template <typename T>
void computeLegJacobianAndPosition(Quadruped<T>& quad, Vec3<T>& q, Mat3<T>* J, Vec3<T>* p) {
    // copy params
    T alpha_h = pi / 2 - q[1];
    T alpha_k = pi / 2 + q[2];
    T gamma = q[0];
    T L1 = quad._shortLinkLength;
    T L2 = quad._longLinkLength;
    T L3 = quad._ExtendLinkLength;

    // compute data
    T beta = (pi - alpha_h - alpha_k) / 2;
    T Loc = cos(beta) * L1 + sqrt(L2 * L2 - sin(beta) * sin(beta) * L1 * L1);
    T b = acos((L1 * L1 + L2 * L2 - Loc * Loc) / (2 * L1 * L2)) - alpha_k;
    T M = Loc * cos(beta + alpha_h) + L3 * cos(b);
    T N = Loc * sin(beta + alpha_h) + L3 * sin(b);

    if (J) {
        J->operator()(0) = 0;
        J->operator()(1) =
                    cos(q[0]) *
            (L3 * cos(q[2] + acos((L1 * cos(q[1] - q[2]) - L1 + pow(2,
                                   (0.5)) * cos(q[1] / 2 - q[2] / 2) *
                                       pow((pow(L1, 2) * cos(q[1] - q[2]) - pow(L1, 2) + 2 * pow(L2, 2)),
                                   (0.5))) /
                                  (2 * L2))) +
             cos(q[1] / 2 + q[2] / 2) *
                 (L1 * cos(q[1] / 2 - q[2] / 2) +
                  (pow(2, (0.5) )*pow((pow(L1, 2) * cos(q[1] - q[2]) - pow(L1, 2) + 2 * pow(L2, 2)), (0.5))) / 2));
        J->operator()(2) =
                    sin(q[0]) *
            (L3 * cos(q[2] + acos((L1 * cos(q[1] - q[2]) - L1 + pow(2,
                                   (0.5)) * cos(q[1] / 2 - q[2] / 2) *
                                       pow((pow(L1, 2) * cos(q[1] - q[2]) - pow(L1, 2) + 2 * pow(L2, 2)),
                                   (0.5))) /
                                  (2 * L2))) +
             cos(q[1] / 2 + q[2] / 2) *
                 (L1 * cos(q[1] / 2 - q[2] / 2) +
                  (pow(2, (0.5) )*pow((pow(L1, 2) * cos(q[1] - q[2]) - pow(L1, 2) + 2 * pow(L2, 2)), (0.5))) / 2));
        J->operator()(3) =
                    -cos(q[1] / 2 + q[2] / 2 - pi / 2) *
                ((L1 * sin(q[1] / 2 - q[2] / 2)) / 2 +
                 (pow(L1, 2) * cos(q[1] / 2 - q[2] / 2) * sin(q[1] / 2 - q[2] / 2)) /
                     (2 * pow((pow(L2, 2) - pow(L1, 2) * pow(sin(q[1] / 2 - q[2] / 2), 2)), (0.5)))) -
            (sin(q[1] / 2 + q[2] / 2 - pi / 2) *
             (L1 * cos(q[1] / 2 - q[2] / 2) + pow((pow(L2, 2) - pow(L1, 2) * pow(sin(q[1] / 2 - q[2] / 2), 2)), (0.5)))) /
                2 -
            (L3 *
             sin(q[2] + pi / 2 -
                 acos((pow(L1, 2) -
                           pow((L1 * cos(q[1] / 2 - q[2] / 2) +pow( (pow(L2, 2) - pow(L1, 2) * pow(sin(q[1] / 2 - q[2] / 2), 2)),
                            (0.5))),
                       2) + pow(L2, 2)) /
                      (2 * L1 * L2))) *
             ((L1 * sin(q[1] / 2 - q[2] / 2)) / 2 +
              (pow(L1, 2) * cos(q[1] / 2 - q[2] / 2) * sin(q[1] / 2 - q[2] / 2)) /
                  (2 * pow((pow(L2, 2) - pow(L1, 2) * pow(sin(q[1] / 2 - q[2] / 2), 2)), (0.5)))) *
             (L1 * cos(q[1] / 2 - q[2] / 2) + pow((pow(L2, 2) - pow(L1, 2) * pow(sin(q[1] / 2 - q[2] / 2), 2)), (0.5)))) /
                (L1 * L2 *
                     pow((1 -pow((pow(L1, 2) - pow((L1 * cos(q[1] / 2 - q[2] / 2) +
                                            pow((pow(L2, 2) - pow(L1, 2) * pow(sin(q[1] / 2 - q[2] / 2), 2)),
                                         (0.5))),
                           2 )+ pow(L2, 2)),
                      2) / (4 * pow(L1, 2) * pow(L2, 2))),
                 (0.5)));
        J->operator()(4) =
                    -sin(q[0]) *
            ((cos(q[1] / 2 + q[2] / 2 - pi / 2) *
              (L1 * cos(q[1] / 2 - q[2] / 2) + pow((pow(L2, 2) - pow(L1, 2) * pow(sin(q[1] / 2 - q[2] / 2), 2)), (0.5)))) /
                 2 -
             sin(q[1] / 2 + q[2] / 2 - pi / 2) *
                 ((L1 * sin(q[1] / 2 - q[2] / 2)) / 2 +
                  (pow(L1, 2) * cos(q[1] / 2 - q[2] / 2) * sin(q[1] / 2 - q[2] / 2)) /
                      (2 * pow((pow(L2, 2) - pow(L1, 2) * pow(sin(q[1] / 2 - q[2] / 2), 2)), (0.5)))) +
             (L3 *
              cos(q[2] + pi / 2 -
                  acos((pow(L1, 2) -
                            pow((L1 * cos(q[1] / 2 - q[2] / 2) +pow((pow(L2, 2) - pow(L1, 2) * pow(sin(q[1] / 2 - q[2] / 2), 2)),
                             (0.5))),
                        2 )+ pow(L2, 2)) /
                       (2 * L1 * L2))) *
              ((L1 * sin(q[1] / 2 - q[2] / 2)) / 2 +
               (pow(L1, 2) * cos(q[1] / 2 - q[2] / 2) * sin(q[1] / 2 - q[2] / 2)) /
                   (2 * pow((pow(L2, 2) - pow(L1, 2) * pow(sin(q[1] / 2 - q[2] / 2), 2)), (0.5)))) *
              (L1 * cos(q[1] / 2 - q[2] / 2) + pow((pow(L2, 2) - pow(L1, 2) * pow(sin(q[1] / 2 - q[2] / 2), 2)), (0.5)))) /
                 (L1 * L2 *
                      pow((1 - pow((pow(L1, 2) - pow((L1 * cos(q[1] / 2 - q[2] / 2) +
                                              pow((pow(L2, 2) - pow(L1, 2) * pow(sin(q[1] / 2 - q[2] / 2), 2)),
                                          (0.5))),
                            2) + pow(L2, 2)),
                       2)/ (4 * pow(L1, 2) * pow(L2, 2))),
                  (0.5))));
        J->operator()(5) =
                    cos(q[0]) *
            ((cos(q[1] / 2 + q[2] / 2 - pi / 2) *
              (L1 * cos(q[1] / 2 - q[2] / 2) + pow((pow(L2, 2) - pow(L1, 2) * pow(sin(q[1] / 2 - q[2] / 2), 2)), (0.5)))) /
                 2 -
             sin(q[1] / 2 + q[2] / 2 - pi / 2) *
                 ((L1 * sin(q[1] / 2 - q[2] / 2)) / 2 +
                  (pow(L1, 2) * cos(q[1] / 2 - q[2] / 2) * sin(q[1] / 2 - q[2] / 2)) /
                      (2 * pow((pow(L2, 2) - pow(L1, 2) * pow(sin(q[1] / 2 - q[2] / 2), 2)), (0.5)))) +
             (L3 *
              cos(q[2] + pi / 2 -
                  acos((pow(L1, 2) -
                            pow((L1 * cos(q[1] / 2 - q[2] / 2) +pow((pow(L2, 2) - pow(L1, 2) * pow(sin(q[1] / 2 - q[2] / 2), 2)),
                             (0.5))),
                        2 )+ pow(L2, 2)) /
                       (2 * L1 * L2))) *
              ((L1 * sin(q[1] / 2 - q[2] / 2)) / 2 +
               (pow(L1, 2) * cos(q[1] / 2 - q[2] / 2) * sin(q[1] / 2 - q[2] / 2)) /
                   (2 * pow((pow(L2, 2) - pow(L1, 2) * pow(sin(q[1] / 2 - q[2] / 2), 2)), (0.5)))) *
              (L1 * cos(q[1] / 2 - q[2] / 2) + pow((pow(L2, 2) - pow(L1, 2) * pow(sin(q[1] / 2 - q[2] / 2), 2)), (0.5)))) /
                 (L1 * L2 *
                      pow((1 - pow((pow(L1, 2) - pow((L1 * cos(q[1] / 2 - q[2] / 2) +
                                              pow((pow(L2, 2) - pow(L1, 2) * pow(sin(q[1] / 2 - q[2] / 2), 2)),
                                          (0.5))),
                            2) + pow(L2, 2)),
                       2)/ (4 * pow(L1, 2) * pow(L2, 2))),
                  (0.5))));
        J->operator()(6) =
                    cos(q[1] / 2 + q[2] / 2 - pi / 2) *
                ((L1 * sin(q[1] / 2 - q[2] / 2)) / 2 +
                 (pow(L1, 2) * cos(q[1] / 2 - q[2] / 2) * sin(q[1] / 2 - q[2] / 2)) /
                     (2 * pow((pow(L2, 2) - pow(L1, 2) * pow(sin(q[1] / 2 - q[2] / 2), 2)), (0.5)))) -
            (sin(q[1] / 2 + q[2] / 2 - pi / 2) *
             (L1 * cos(q[1] / 2 - q[2] / 2) + pow((pow(L2, 2) - pow(L1, 2) * pow(sin(q[1] / 2 - q[2] / 2), 2)), (0.5)))) /
                2 +
            L3 *
                sin(q[2] + pi / 2 -
                    acos((pow(L1, 2) - pow((L1 * cos(q[1] / 2 - q[2] / 2) +
                                            pow((pow(L2, 2) - pow(L1, 2) * pow(sin(q[1] / 2 - q[2] / 2), 2)),
                                        (0.5))),
                          2) + pow(L2, 2)) /
                         (2 * L1 * L2))) *
                ((((L1 * sin(q[1] / 2 - q[2] / 2)) / 2 +
                   (pow(L1, 2) * cos(q[1] / 2 - q[2] / 2) * sin(q[1] / 2 - q[2] / 2)) /
                       (2 * pow((pow(L2, 2) - pow(L1, 2) * pow(sin(q[1] / 2 - q[2] / 2), 2)), (0.5)))) *
                  (L1 * cos(q[1] / 2 - q[2] / 2) + pow((pow(L2, 2) - pow(L1, 2) * pow(sin(q[1] / 2 - q[2] / 2), 2)), (0.5)))) /
                     (L1 * L2 *
                          pow((1 - pow((pow(L1, 2) - pow((L1 * cos(q[1] / 2 - q[2] / 2) +
                                                  pow((pow(L2, 2) - pow(L1, 2) * pow(sin(q[1] / 2 - q[2] / 2), 2)),
                                              (0.5))),
                                2 )+ pow(L2, 2)),
                           2) / (4 * pow(L1, 2) * pow(L2, 2))) ,
                      (0.5))) -
                 1);
        J->operator()(7) =
                    -sin(q[0]) *
            ((cos(q[1] / 2 + q[2] / 2 - pi / 2) *
              (L1 * cos(q[1] / 2 - q[2] / 2) + pow((pow(L2, 2) - pow(L1, 2) * pow(sin(q[1] / 2 - q[2] / 2), 2)), (0.5)))) /
                 2 +
             sin(q[1] / 2 + q[2] / 2 - pi / 2) *
                 ((L1 * sin(q[1] / 2 - q[2] / 2)) / 2 +
                  (pow(L1, 2) * cos(q[1] / 2 - q[2] / 2) * sin(q[1] / 2 - q[2] / 2)) /
                      (2 * pow((pow(L2, 2) - pow(L1, 2) * pow(sin(q[1] / 2 - q[2] / 2), 2)), (0.5)))) -
             L3 *
                 cos(q[2] + pi / 2 -
                     acos((pow(L1, 2) - pow((L1 * cos(q[1] / 2 - q[2] / 2) +
                                            pow((pow(L2, 2) - pow(L1, 2) * pow(sin(q[1] / 2 - q[2] / 2), 2)),
                                         (0.5))),
                           2 )+ pow(L2, 2)) /
                          (2 * L1 * L2))) *
                 ((((L1 * sin(q[1] / 2 - q[2] / 2)) / 2 +
                    (pow(L1, 2) * cos(q[1] / 2 - q[2] / 2) * sin(q[1] / 2 - q[2] / 2)) /
                        (2 * pow((pow(L2, 2) - pow(L1, 2) * pow(sin(q[1] / 2 - q[2] / 2), 2)), (0.5)))) *
                   (L1 * cos(q[1] / 2 - q[2] / 2) + pow((pow(L2, 2) - pow(L1, 2) * pow(sin(q[1] / 2 - q[2] / 2), 2)), (0.5)))) /
                      (L1 * L2 *
                           pow((1 - pow((pow(L1, 2) - pow((L1 * cos(q[1] / 2 - q[2] / 2) +
                                                   pow((pow(L2, 2) - pow(L1, 2) * pow(sin(q[1] / 2 - q[2] / 2), 2)),
                                               (0.5))),
                                 2) + pow(L2, 2)),
                            2) / (4 * pow(L1, 2) * pow(L2, 2))),
                       (0.5))) -
                  1));
        J->operator()(8) =
                cos(q[0]) *
            ((cos(q[1] / 2 + q[2] / 2 - pi / 2) *
              (L1 * cos(q[1] / 2 - q[2] / 2) + pow((pow(L2, 2) - pow(L1, 2) * pow(sin(q[1] / 2 - q[2] / 2), 2)), (0.5)))) /
                 2 +
             sin(q[1] / 2 + q[2] / 2 - pi / 2) *
                 ((L1 * sin(q[1] / 2 - q[2] / 2)) / 2 +
                  (pow(L1, 2) * cos(q[1] / 2 - q[2] / 2) * sin(q[1] / 2 - q[2] / 2)) /
                      (2 * pow((pow(L2, 2) - pow(L1, 2) * pow(sin(q[1] / 2 - q[2] / 2), 2)), (0.5)))) -
             L3 *
                 cos(q[2] + pi / 2 -
                     acos((pow(L1, 2) - pow((L1 * cos(q[1] / 2 - q[2] / 2) +
                                            pow((pow(L2, 2) - pow(L1, 2) * pow(sin(q[1] / 2 - q[2] / 2), 2)),
                                         (0.5))),
                           2 )+ pow(L2, 2)) /
                          (2 * L1 * L2))) *
                 ((((L1 * sin(q[1] / 2 - q[2] / 2)) / 2 +
                    (pow(L1, 2) * cos(q[1] / 2 - q[2] / 2) * sin(q[1] / 2 - q[2] / 2)) /
                        (2 * pow((pow(L2, 2) - pow(L1, 2) * pow(sin(q[1] / 2 - q[2] / 2), 2)), (0.5)))) *
                   (L1 * cos(q[1] / 2 - q[2] / 2) + pow((pow(L2, 2) - pow(L1, 2) * pow(sin(q[1] / 2 - q[2] / 2), 2)), (0.5)))) /
                      (L1 * L2 *
                           pow((1 - pow((pow(L1, 2) - pow((L1 * cos(q[1] / 2 - q[2] / 2) +
                                                   pow((pow(L2, 2) - pow(L1, 2) * pow(sin(q[1] / 2 - q[2] / 2), 2)),
                                               (0.5))),
                                 2) + pow(L2, 2)),
                            2) / (4 * pow(L1, 2) * pow(L2, 2))),
                       (0.5))) -
                  1));
    }

    if (p) {
        p->operator()(0) = M;
        p->operator()(1) = sin(gamma) * N;
        p->operator()(2) = -cos(gamma) * N;
    }
}

template void computeAuxiliaryAngle<float>(Quadruped<float>& quad, Vec3<float>& p, Vec3<float>* q);
template void computeAuxiliaryAngle<double>(Quadruped<double>& quad, Vec3<double>& p, Vec3<double>* q);

template void computeLegAngle<float>(Quadruped<float>& quad, Vec3<float>& p, Vec3<float>* q);
template void computeLegAngle<double>(Quadruped<double>& quad, Vec3<double>& p, Vec3<double>* q);

template void computeLegJacobianAndPosition<double>(Quadruped<double>& quad, Vec3<double>& q, Mat3<double>* J,
                                                    Vec3<double>* p);
template void computeLegJacobianAndPosition<float>(Quadruped<float>& quad, Vec3<float>& q, Mat3<float>* J,
                                                   Vec3<float>* p);

/************************ COPYRIGHT(C) SCUT-RobotLab Development Team **************************/
